#include "body.h"

namespace eqPc{

#pragma region Body
	Body::Body(BodyType type) : _type(type)
	{}

	void Body::update(const eq::Matrix4f& matrix, float quality)
	{
		_quality = quality;
		if(quality < 0){
			_state = TrackingState::UNTRACKED;
			return;
		}
		
		_state = TrackingState::TRACKED;
		_matrix = matrix;
	}	
#pragma endregion Body

#pragma region Head

	Head::Head() : Body(BodyType::HEAD)
	{
		_state = TrackingState::UNTRACKED;
		_matrix = eq::Matrix4f::IDENTITY;
		
		_quality = -1.0f;
	}

#pragma endregion Head

#pragma region FlyStick

	FlyStick::FlyStick() : Body(BodyType::FLYSTICK)
	{
		_state = TrackingState::UNTRACKED;
		_matrix = eq::Matrix4f::IDENTITY;
		_triggerMatrix = eq::Matrix4f::IDENTITY;
		_quality = -1.0f;

		for(int i=0;i<5;i++)
			_buttons[i]=_oldButtons[i]=ButtonState::BUTTON_UNPRESSED;

		_joyStickValue[0]=_joyStickValue[1]=0.0f;
	}

	void FlyStick::update(const eq::Matrix4f& matrix, float quality, int *buttons, float *joystick)
	{
		Body::update(matrix,quality);

		_joyStickValue[0] = joystick[0];_joyStickValue[1] = joystick[1];

		_handleButtonState(buttons);		

		if(getButtonState(Button::BUTTON_TRIGGER)==ButtonState::BUTTON_TRIGGERED)
			_triggerMatrix = _matrix;
		if(getButtonState(Button::BUTTON_TRIGGER)==ButtonState::BUTTON_RELEASED)
			_triggerMatrix  = eq::Matrix4f::IDENTITY;

		
	}	

	//FSM for button handling
	//States: UNPRESSED, PRESSED, TRIGGERED, RELEASE
	void FlyStick::_handleButtonState(int *buttons)
	{
		for(int i=0;i<5;i++){
			if(_buttons[i]==ButtonState::BUTTON_UNPRESSED){
				if(buttons[i]==1)
					_buttons[i]=ButtonState::BUTTON_TRIGGERED;
				else
					_buttons[i]=ButtonState::BUTTON_UNPRESSED;
			}
			if(_buttons[i]==ButtonState::BUTTON_TRIGGERED){
				if(buttons[i]==1)
					_buttons[i]=ButtonState::BUTTON_PRESSED;
				else
					_buttons[i]=ButtonState::BUTTON_UNPRESSED;
			}
			if(_buttons[i]==ButtonState::BUTTON_PRESSED){
				if(buttons[i]==1)
					_buttons[i]=ButtonState::BUTTON_PRESSED;
				else
					_buttons[i]=ButtonState::BUTTON_RELEASED;
			}
			if(_buttons[i]==ButtonState::BUTTON_RELEASED){
				if(buttons[i]==1)
					_buttons[i]=ButtonState::BUTTON_PRESSED;
				else
					_buttons[i]=ButtonState::BUTTON_UNPRESSED;
			}

		}
		/*for(int i=0;i<5;i++){
			if( _buttons[i]==ButtonState::BUTTON_UNPRESSED && _oldButtons[i]==ButtonState::BUTTON_UNPRESSED)
			{
				if(buttons[i] == 1){
					_oldButtons[i]=_buttons[i];
					_buttons[i]=ButtonState::BUTTON_TRIGGERED;

				}
				break;
			}
			if( _buttons[i]==ButtonState::BUTTON_TRIGGERED && _oldButtons[i]==ButtonState::BUTTON_UNPRESSED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_PRESSED;

				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_RELEASED;
				}
				break;
			}
			if( _buttons[i]==ButtonState::BUTTON_PRESSED && _oldButtons[i]==ButtonState::BUTTON_TRIGGERED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_PRESSED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_RELEASED;
				}
				break;
			}			
			if( _buttons[i]==ButtonState::BUTTON_PRESSED && _oldButtons[i]==ButtonState::BUTTON_PRESSED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_PRESSED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_RELEASED;
				}
				break;
			}
			if( _buttons[i]==ButtonState::BUTTON_RELEASED && _oldButtons[i]==ButtonState::BUTTON_PRESSED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_TRIGGERED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_UNPRESSED;
				}
				break;
			}
			if( _buttons[i]==ButtonState::BUTTON_RELEASED && _oldButtons[i]==ButtonState::BUTTON_TRIGGERED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_TRIGGERED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_UNPRESSED;
				}
				break;
			}	
			if( _buttons[i]==ButtonState::BUTTON_TRIGGERED && _oldButtons[i]==ButtonState::BUTTON_RELEASED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_PRESSED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_RELEASED;
				}
				break;
			}	
			if( _buttons[i]==ButtonState::BUTTON_UNPRESSED && _oldButtons[i]==ButtonState::BUTTON_RELEASED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_TRIGGERED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_UNPRESSED;
				}
				break;
			}
			if( _buttons[i]==ButtonState::BUTTON_RELEASED && _oldButtons[i]==ButtonState::BUTTON_TRIGGERED)
			{
				if(buttons[i] == 1){
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_TRIGGERED;
				}
				else{
					_oldButtons[i] = _buttons[i];
					_buttons[i] = ButtonState::BUTTON_UNPRESSED;
				}
				break;
			}		
		}*/
	
	}
	
	const eq::Matrix4f& FlyStick::getDeltaMatrix()
	{
		eq::Matrix4f delta = eq::Matrix4f::IDENTITY;

		if(getButtonState(Button::BUTTON_TRIGGER) != ButtonState::BUTTON_PRESSED)
			return delta;

		eq::Matrix4f triggered_trans = eq::Matrix4f::IDENTITY;
		_triggerMatrix.transpose_to(triggered_trans);

		delta = triggered_trans * _matrix;
	
		delta.array[3]=delta.array[7]=delta.array[11]=delta.array[12]=delta.array[13]=delta.array[14]=0;
		delta.array[15]=1;

		eq::Matrix4f delta2;
		delta.transpose_to(delta2);

		delta2.x() = _matrix.x() - _triggerMatrix.x();
		delta2.y() = _matrix.y() - _triggerMatrix.y();
		delta2.z() = _matrix.z() - _triggerMatrix.z();

		return delta2;
	}




#pragma endregion FlyStick
}